/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * utilities.h
 *
 * PURPOSE:
 *   System utilities functions
 *
 * CREATED:
 * 2/2009 by Cory Dixon
 *
 ***********************************************************************/

#ifndef __UTILITIES_H
#define __UTILITIES_H

#include <math.h>
#include <vector>
#include <errno.h>

#include "matrix.h"
#include "String.h"
#include "commandStruct.h"

/* some multipliers for common units */
#define METERS_TO_FEET  3.2808399   /* Meters to U.S./British feet */
#define METERS_TO_MILES 0.00062137119  /* Meters to miles */
#define METERS_TO_NMILES 0.000539956803  /* Meters to nautical miles */

#define FEET_TO_METERS  0.3048
#define FEET_TO_MILES   0.000189393939 
#define FEET_TO_NMILES  0.000164578834

#define MILES_TO_METES 1609.344  /* Meters to miles */
#define MILES_TO_FEET  5280.0  /* Meters to miles */

#define KNOTS_TO_MPH 1.15077945   /* Knots to miles per hour */
#define KNOTS_TO_KPH 1.852    /* Knots to kilometers per hour */
#define KNOTS_TO_FPS 1.68780986    /* Knots to feet per second */
#define KNOTS_TO_MPS 0.51444444  /* Knots to meters per second */

#define MPH_TO_MPS   0.44704      /* Meters per second to klicks/hr */
#define MPH_TO_KPH   1.609344      /* Meters per second to klicks/hr */
#define MPH_TO_FPS   1.46666667   /* Meters per second to feet per second */
#define MPH_TO_KNOTS 0.868976242  /* Meters per second to knots */

#define MPS_TO_KPH   3.6      /* Meters per second to klicks/hr */
#define MPS_TO_FPS   3.2808399   /* Meters per second to feet per second */
#define MPS_TO_MPH   2.2369363   /* Meters/second to miles per hour */
#define MPS_TO_KNOTS 1.94384449  /* Meters per second to knots */

/* angle conversion multipliers */
//#define PI        3.1415926535897932384626433832795029l
#define PI_2      6.283185307179586l
#define RAD_2_DEG 57.2957795130823208767981548141051703l
#define DEG_2_RAD 0.0174532925199432957692369076848861271l

typedef enum {
	SI_M_U,       // base unit of meters
	SI_KM_U,      // base unit of kilo-meters
	ENGLISH_F_U,  // base unit of feet
	ENGLISH_M_U,  // base unit of mile
	ENGLISH_NM_U, // base unit of nautical mile
} UnitSystem_t;

double meterToLengthU(double m, UnitSystem_t unit);
double feetToLengthU(double m, UnitSystem_t unit);
double mpsToSpeedU(double mps, UnitSystem_t unit);
double mphToSpeedU(double m, UnitSystem_t unit);
double ktsToSpeedU(double m, UnitSystem_t unit);

const char *lengthU_str(UnitSystem_t unit, double range = NAN);
const char *speedU_str(UnitSystem_t unit);

//-------------------- text string ----------------------------//
char * replace_string(const char *src, const char *from, const char *to);

//-------------------- GPS <-> distance ----------------------------//

// Calculate the distance [m] between two gps lat/lon coordinates, note
// this assumes a flat earth model and no altitude seperation.
double GPSRange(GPSLLA_t gps1, GPSLLA_t gps2);
double GPSDist(GPSLLA_t gps1, GPSLLA_t gps2);
double GPSBearing(GPSLLA_t gps1, GPSLLA_t gps2);

// Some useful macros for geo lat-lon to meters
//    d_LAT -  differene in degrees latitude to meters
//#define LAT_TO_M(d_LAT)     ((d_LAT) / 0.000009)
//#define M_TO_LAT(METERS)    ((METERS) * 0.000009)
#define LAT_TO_M(d_LAT)     ((d_LAT) * 111120.0);
#define M_TO_LAT(METERS)    ((METERS) / 111120.0);

//    LAT   -  Actual lattidue in deg
//    d_LON -  differene in degrees longitude to meters
#define LON_TO_M(d_LON,LAT)  ((d_LON) / 0.000009 * cos((LAT)*M_PI/180.0))
#define M_TO_LON(METERS,LAT) ((METERS) * 0.000009 / (cos((LAT)*M_PI/180.0)))

//  convert latitude [deg], longitude [deg], and altitude [m] to
//  earth-centered, earth-fixed (ECEF) cartesian [m]
CartVector_t lla2ecef(GPSLLA_t &gps);
CartVector_t lla2local(GPSLLA_t &gps, GPSLLA_t &datum);
GPSLLA_t     ecef2lla(CartVector_t &ecef);

Matrix lla2ecef(Matrix &lla); // lat->lla(0), lon->lla(1), alt->lla(2)
Matrix ecef2lla(Matrix &ecef);
Matrix ecef2nedDCM(GPSLLA_t gps);
Matrix ecef2nedDCM(Matrix &lla);

//-------------------- Data Filtering ----------------------------//
// Exponential weighted moving average smoother
//     past    - past estimate
//     measure - current measurement
//     alpha   - smoothing factor in the range [0,1] 0->no update, 1->no smoothing
//
float smoothed(float past, float measure, float alpha);

// Round a double value to an integer
//     value - value to round
int Round(double Value);

inline bool IsNaN(double val)
{
	return val != val;
}

//-- Digital Butterworth 5th order filter 
//  make sure to set all values to NAN of xv and yv
//  when you initialize
#define BUTTER_NZEROS 5
#define BUTTER_NPOLES 5
#define BUTTER_GAIN   7.796778047e+02
float butter_filter_5(float val, float xv[BUTTER_NZEROS+1], float yv[BUTTER_NPOLES+1]);

//-------------------- File I/O ----------------------------//
// Open a numbered file from a directory using the base file name
//     dir     - directory for which to generage file name
//     base    - base file name, e.g. rssi_trace_
//     ext     - extension type of file , e.g. txt, dat, log
//
String generteNumberedFile( String dir, String base, String ext = "txt" );

//-------------------- Orbit Controller ----------------------------//
typedef struct{
	float x;
	float y;
} Velocity_t;

float getVelocityRate(float phase, float partner_phase, float k_p) ;
float getTurnRate(float psi, float psides, float psidotdes, float k_p) ;
float getTurnRate(float psi,  float psidot, float psides, float k_p, float k_d);
void vectorField(GPSLLA_t vehicle, float vehicle_heading, GPSLLA_t target, 
                 float rdes, float speed, Velocity_t* vdes, float* psides, float* psidotdes);

string getFileName(const string &str);
int getdir (string dir, vector<string> &files);

#endif // __UTILITIES_H
